#include "S_MPU9250.h"
#include "S_SYSTICK.h"

int16_t g_offset[3];
int16_t a_offset[3];

uint8_t mpu9250_init(void)
{
	if (mpu9250_readID() == 0x71)
	{
		uint8_t buf[6];

		uint8_t reset = 0x80;
		uint8_t wakeup = 0x00;
		uint8_t disableint = 0x00;
		uint8_t gyrocfg = 0b00010000;
		uint8_t acclcfg = 0b00001000;
		uint8_t cfg = MPU9250_DLPF_BW_20;
		uint8_t smpdiv = 0x00;
		uint8_t intpincfg = 0x02;

		uint8_t ak8963cfg = 0x11;

		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_PWR_MGMT_1, 1, &reset, 1, 0xff); //复位

		HAL_Delay(100);

		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_PWR_MGMT_1, 1, &wakeup, 1, 0xff);	 //唤醒
		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_INT_ENABLE, 1, &disableint, 1, 0xff); //禁止中断
		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_GYRO_CONFIG, 1, &gyrocfg, 1, 0xff);	 //陀螺仪高精度
		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_ACCEL_CONFIG, 1, &acclcfg, 1, 0xff);	 //加速度计高精度
		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_CONFIG, 1, &cfg, 1, 0xff);			 //设置陀螺仪的输出1Khz
		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_SMPLRT_DIV, 1, &smpdiv, 1, 0xff);	 //采样分频
		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_INT_PIN_CFG, 1, &intpincfg, 1, 0xff); //MPU可以直接访问磁力计

		HAL_I2C_Mem_Write(&hi2c1, AK8963Addr, 0x0A, 1, &ak8963cfg, 1, 0xff);

		HAL_I2C_Mem_Read(&hi2c1, MPU9250Addr, MPU9250_XA_OFFSET_H, 1, buf, 6, 0xff);
		for (uint8_t i = 0; i < 3; i++)
			a_offset[i] = (int16_t)((buf[2 * i] << 7) | buf[2 * i + 1]);

		HAL_I2C_Mem_Read(&hi2c1, MPU9250Addr, MPU9250_XG_OFFSET_H, 1, buf, 6, 0xff);
		for (uint8_t i = 0; i < 3; i++)
			g_offset[i] = (int16_t)((buf[2 * i] << 7) | buf[2 * i + 1]);

		return 1;
	}
	else
		return 0;
}

uint8_t mpu9250_readID(void)
{
	uint8_t id;
	HAL_I2C_Mem_Read(&hi2c1, MPU9250Addr, MPU9250_WHO_AM_I, 1, &id, 1, 0xff);
	return id;
}

void MPU9250read(int16_t *accdata, int16_t *gyrodata, int16_t *magdata)
{
	uint8_t buffer[6];
	HAL_I2C_Mem_Read(&hi2c1, MPU9250Addr, MPU9250_ACCEL_XOUT_H, 1, buffer, 6, 0xff);
	for (uint8_t i = 0; i < 3; i++)
		accdata[i] = (int16_t)((buffer[2 * i] << 8) | buffer[2 * i + 1]) - a_offset[i];

	HAL_I2C_Mem_Read(&hi2c1, MPU9250Addr, MPU9250_GYRO_XOUT_H, 1, buffer, 6, 0xff);
	for (uint8_t i = 0; i < 3; i++)
		gyrodata[i] = (int16_t)((buffer[2 * i] << 8) | buffer[2 * i + 1]) - g_offset[i];

	HAL_I2C_Mem_Read(&hi2c1, AK8963Addr, AK8963_MAG_OUT_L, 1, buffer, 6, 0xff);
	for (uint8_t i = 0; i < 3; i++)
		magdata[i] = (int16_t)((buffer[2 * i + 1] << 8) | buffer[2 * i]);
}


uint8_t MPU_i2cWrite(uint8_t slave_addr,uint8_t reg_addr,uint8_t length,uint8_t *data)
{
	uint32_t status = HAL_I2C_Mem_Write(&hi2c1,slave_addr<<1,reg_addr,1,data,length,0xff);
	if(status!=HAL_OK)
	UART_printf(&huart1,"i2cwriteerror!");
	return status;
}

uint8_t MPU_i2cRead(uint8_t slave_addr,uint8_t reg_addr,uint8_t length,uint8_t *data)
{
	uint32_t status = HAL_I2C_Mem_Read(&hi2c1,slave_addr<<1,reg_addr,1,data,length,0xff);
	if(status!=HAL_OK)
	return status; 
}

void mpl_getms(uint32_t *num)
{
	*num = Get_Systick();
}
void mpl_printf(const char *fmt, ...)
{
	va_list ap;
	va_start(ap, fmt);
	int length;
	char buffer[128];
	length = vsnprintf(buffer, 128, fmt, ap);
	HAL_UART_Transmit(&huart1, (uint8_t *)buffer, length, 0xffff);
}